#include <Arduino.h>
#include "led_control.h"
#include "config.h"
#include "s3_motor.h"

// 创建LED控制对象
LedControl led(LED_PIN);

// 系统运行时间记录（毫秒）
unsigned long systemStartTime = 0;

// 电机测试模式变量
bool motorTestMode = false;
unsigned long lastStepTime = 0;
int testStepCount = 0;
#define MAX_TEST_STEPS 20  // 测试模式下的最大步数

// 标签定位模式变量
bool labelLocatingMode = false;  // 标签定位模式标志
int initialAdcValue = 0;         // 初始ADC值
int minAdcValue = 0;             // 最小ADC值（缝隙处）
int currentAdcValue = 0;         // 当前ADC值
bool edgeDetected = false;       // 是否检测到标签边缘

// 定义一些常量
#define MAX_COMMAND_LENGTH 10
char commandBuffer[MAX_COMMAND_LENGTH];
int commandIndex = 0;
bool commandComplete = false;

// 运行统计信息
uint32_t totalSteps = 0;         // 总步数
uint32_t commandCounter = 0;     // 总命令数

// 显示帮助信息
void showHelp() {
  Serial.println("\n可用命令列表:");
  Serial.println("r        - 电机前进80步");
  Serial.println("s        - 停止电机");
  Serial.println("m [步数] - 电机前进指定步数 (范围: 1-1000)");
  Serial.println("t        - 测试模式：每秒移动一步，观察电机状态");
  Serial.println("l        - 切换LED状态");
  Serial.println("b [次数] - LED闪烁指定次数 (范围: 1-10)");
  Serial.println("f        - 进入标签定位模式，自动寻找标签上边缘");
  Serial.println("i        - 显示系统信息");
  Serial.println("h        - 显示此帮助信息");
  Serial.println();
}

// 显示系统信息
void showSystemInfo() {
  unsigned long currentTime = millis();
  unsigned long runTime = currentTime - systemStartTime;
  
  Serial.println("\n系统信息:");
  Serial.print("设备: ");
  Serial.println(DEVICE_NAME);
  Serial.print("固件版本: ");
  Serial.println(FIRMWARE_VERSION);
  
  // 显示运行时间
  unsigned long seconds = runTime / 1000;
  unsigned long minutes = seconds / 60;
  unsigned long hours = minutes / 60;
  unsigned long days = hours / 24;
  
  seconds %= 60;
  minutes %= 60;
  hours %= 24;
  
  Serial.print("运行时间: ");
  if (days > 0) {
    Serial.print(days);
    Serial.print("天 ");
  }
  Serial.print(hours);
  Serial.print("时 ");
  Serial.print(minutes);
  Serial.print("分 ");
  Serial.print(seconds);
  Serial.println("秒");
  
  // 显示统计数据
  Serial.print("总命令数: ");
  Serial.println(commandCounter);
  Serial.print("总步进数: ");
  Serial.println(totalSteps);
  
  // 显示LED状态
  Serial.print("LED状态: ");
  Serial.println(led.getState() ? "开启" : "关闭");
  
  // 显示USB状态信息
  Serial.print("USB CDC状态: ");
  #ifdef ARDUINO_USB_CDC_ON_BOOT
    Serial.println("已启用");
  #else
    Serial.println("未启用");
  #endif
  
  Serial.println();
}

// 读取传感器ADC值
int readLabelSensor() {
  return analogRead(PIN_LABEL_SENSOR);
}

// 标签定位模式初始化
void startLabelLocating() {
  Serial.println("进入标签定位模式");
  Serial.println("系统将自动寻找标签上边缘，请确保纸张已正确放置");
  
  // 确保电机处于关闭状态开始
  motor_stop();
  
  // 读取初始ADC值
  initialAdcValue = readLabelSensor();
  minAdcValue = initialAdcValue;  // 初始化最小值
  
  Serial.print("初始ADC值: ");
  Serial.println(initialAdcValue);
  
  // 重置变量
  edgeDetected = false;
  
  // 开启定位模式
  labelLocatingMode = true;
  
  // 开启LED指示
  led.turnOn();
}

// 串口指令处理函数
void processCommand() {
  // 去除命令末尾的回车和换行符
  if (commandIndex > 0 && (commandBuffer[commandIndex-1] == '\r' || commandBuffer[commandIndex-1] == '\n')) {
    commandIndex--;
  }
  
  // 确保命令字符串以null结尾
  commandBuffer[commandIndex] = '\0';
  
  // 如果命令为空，则直接返回
  if (commandIndex == 0) {
    Serial.print("> ");
    return;
  }
  
  Serial.print("执行命令: ");
  Serial.println(commandBuffer);
  
  // 增加命令计数
  commandCounter++;
  
  // 解析命令
  switch (commandBuffer[0]) {
    case 'r':  // 当接收到'r'指令时
      Serial.println("电机前进80步");
      led.turnOn();  // 开启LED指示
      motor_run_step(80);  // 电机前进80步
      motor_stop();   // 运行结束后关闭电机
      led.turnOff();  // 关闭LED指示
      totalSteps += 80;  // 更新总步数
      Serial.println("电机运行完成");
      break;
    
    case 's':  // 停止电机
      Serial.println("停止电机");
      motor_stop();
      
      // 如果正在执行标签定位模式，也停止它
      if (labelLocatingMode) {
        labelLocatingMode = false;
        led.turnOff();
        Serial.println("标签定位模式已停止");
      }
      
      Serial.println("电机已停止");
      break;
    
    case 'm':  // 电机前进指定步数 (格式: m 100)
      if (commandIndex > 1 && commandBuffer[1] == ' ') {
        int steps = atoi(&commandBuffer[2]);
        if (steps > 0 && steps <= 1000) {
          Serial.print("电机前进 ");
          Serial.print(steps);
          Serial.println(" 步");
          led.turnOn();
          motor_run_step(steps);
          motor_stop();   // 运行结束后关闭电机
          led.turnOff();
          totalSteps += steps;  // 更新总步数
          Serial.println("电机运行完成");
        } else {
          Serial.println("错误: 步数必须在1-1000之间");
        }
      } else {
        Serial.println("错误: 命令格式应为 'm 步数'");
      }
      break;
    
    case 't':  // 测试模式：每秒移动一步，观察电机状态
      if (motorTestMode) {
        Serial.println("测试模式已停止");
        motorTestMode = false;
      } else {
        Serial.println("测试模式已启动");
        motorTestMode = true;
        lastStepTime = millis();
        testStepCount = 0;
      }
      break;
    
    case 'l':  // 切换LED状态
      if (led.getState()) {
        Serial.println("关闭LED");
        led.turnOff();
      } else {
        Serial.println("打开LED");
        led.turnOn();
      }
      break;
      
    case 'f':  // 标签定位模式
      if (labelLocatingMode) {
        Serial.println("已在标签定位模式中");
      } else {
        startLabelLocating();
      }
      break;
    
    case 'b':  // LED闪烁指定次数 (格式: b 5)
      if (commandIndex > 1 && commandBuffer[1] == ' ') {
        int times = atoi(&commandBuffer[2]);
        if (times > 0 && times <= 10) {
          Serial.print("LED闪烁 ");
          Serial.print(times);
          Serial.println(" 次");
          led.blink(times, BLINK_FAST_DELAY, BLINK_FAST_DELAY);
        } else {
          Serial.println("错误: 闪烁次数必须在1-10之间");
        }
      } else {
        Serial.println("错误: 命令格式应为 'b 次数'");
      }
      break;
    
    case 'i':  // 显示系统信息
      showSystemInfo();
      break;
    
    case 'h':  // 显示帮助信息
      showHelp();
      break;
    
    default:
      Serial.print("未知命令: ");
      Serial.println(commandBuffer);
      Serial.println("输入 'h' 查看可用命令");
      break;
  }
  
  // 重置命令缓冲区
  commandIndex = 0;
  Serial.print("> ");
}

void setup() {
  // 记录系统启动时间
  systemStartTime = millis();
  
  // 初始化传感器引脚
  pinMode(PIN_LABEL_SENSOR, INPUT);
  
  // 初始化串口通信
  Serial.begin(SERIAL_BAUD);
  
  // 等待USB CDC串口连接（如果启用）
  #if defined(ARDUINO_USB_CDC_ON_BOOT)
    // 等待串口连接，最多等待3秒
    unsigned long startTime = millis();
    while (!Serial && (millis() - startTime < 3000)) {
      delay(100);
    }
  #else
    delay(1000);  // 普通串口延时
  #endif
  
  // 输出设备信息
  Serial.println("\n==================================");
  Serial.print("设备: ");
  Serial.println(DEVICE_NAME);
  Serial.print("固件版本: ");
  Serial.println(FIRMWARE_VERSION);
  
  // 显示USB CDC状态
  #ifdef ARDUINO_USB_CDC_ON_BOOT
    Serial.println("USB CDC串口: 已启用");
  #endif
  
  Serial.println("==================================");
  Serial.println("系统启动中...");
  
  // 初始化LED
  led.begin();
  init_motor();
  // 启动时闪烁指定次数表示初始化成功
  led.blink(STARTUP_BLINK_COUNT, BLINK_FAST_DELAY, BLINK_FAST_DELAY);
  
  Serial.println("系统启动完成！");
  showHelp();
  Serial.print("> ");
}

void loop() {
  // 检查是否有串口数据可读
  while (Serial.available() > 0) {
    char incomingByte = Serial.read();
    
    // 处理回车键，表示命令完成
    if (incomingByte == '\r' || incomingByte == '\n') {
      if (commandIndex > 0) {
        commandComplete = true;
      }
    } 
    // 处理退格键
    else if (incomingByte == '\b' || incomingByte == 127) {
      if (commandIndex > 0) {
        commandIndex--;
        // 在终端上显示退格效果
        Serial.print("\b \b");
      }
    }
    // 存储其他有效字符
    else if (commandIndex < MAX_COMMAND_LENGTH - 1) {
      commandBuffer[commandIndex++] = incomingByte;
      // 回显字符
      Serial.write(incomingByte);
    }
  }
  
  // 如果命令完成，则处理它
  if (commandComplete) {
    Serial.println(); // 新行
    processCommand();
    commandComplete = false;
  }
  
  // 标签定位模式
  if (labelLocatingMode && !edgeDetected) {
    // 读取当前ADC值
    currentAdcValue = readLabelSensor();
    
    // 输出调试信息
    static unsigned int counter = 0;
    if (counter++ % 5 == 0) {
      // 每5次循环打印一次，避免输出太多
      Serial.print("当前ADC值: ");
      Serial.print(currentAdcValue);
      Serial.print(", 最小ADC值: ");
      Serial.println(minAdcValue);
    }
    
    // 每次读取后与最小值比较，如果更小则更新最小值
    if (currentAdcValue < minAdcValue) {
      minAdcValue = currentAdcValue;
      Serial.print("找到更小的ADC值: ");
      Serial.println(minAdcValue);
    }
    
    // 检测上升沿：当前值大于最小值的1.5倍，说明检测到标签边缘
    if (currentAdcValue > (minAdcValue * 1.5)) {
      Serial.println("\n检测到标签上边缘!");
      Serial.print("最小ADC值: ");
      Serial.print(minAdcValue);
      Serial.print(", 当前ADC值: ");
      Serial.println(currentAdcValue);
      Serial.print("比率: ");
      Serial.println((float)currentAdcValue / minAdcValue);
      
      // 标记为已检测到边缘
      edgeDetected = true;
      
      // 停止电机并确认停止
      motor_stop();
      
      // 闪烁LED指示成功
      led.blink(3, BLINK_FAST_DELAY, BLINK_FAST_DELAY);
      
      // 延时一下，避免反复触发
      delay(500);
      
      // 退出标签定位模式
      labelLocatingMode = false;
      
      Serial.println("标签定位完成，电机已停止");
    } else {
      // 没有检测到上升沿，继续移动电机一小步
      motor_run_step(1);  // 这个函数已修改为自动关闭电机
      totalSteps++;
      
      // 较长延时，让传感器读数稳定，减慢寻找速度
      delay(20);
    }
  }
  
  // 测试模式：每秒移动电机一步，观察电机状态
  if (motorTestMode) {
    unsigned long currentTime = millis();
    
    // 每秒移动一步
    if (currentTime - lastStepTime >= 1000) {
      lastStepTime = currentTime;
      
      // 显示步数信息
      Serial.print("测试步数: ");
      Serial.print(++testStepCount);
      Serial.print(" - 马达位置: ");
      Serial.print(motor_pos);
      
      // 移动一步
      digitalWrite(PIN_MOTOR_AN, motor_table[motor_pos][0]);
      digitalWrite(PIN_MOTOR_BP, motor_table[motor_pos][1]);
      digitalWrite(PIN_MOTOR_AP, motor_table[motor_pos][2]);
      digitalWrite(PIN_MOTOR_BN, motor_table[motor_pos][3]);
      
      // 显示当前引脚状态
      Serial.print(" - 引脚状态 [AN:"); 
      Serial.print(motor_table[motor_pos][0]);
      Serial.print(" BP:");
      Serial.print(motor_table[motor_pos][1]);
      Serial.print(" AP:");
      Serial.print(motor_table[motor_pos][2]);
      Serial.print(" BN:");
      Serial.print(motor_table[motor_pos][3]);
      Serial.println("]");
      
      // 更新电机位置
      motor_pos++;
      if (motor_pos >= 4) {
        motor_pos = 0;
      }
      
      totalSteps++;
      
      // 达到最大测试步数后自动停止
      if (testStepCount >= MAX_TEST_STEPS) {
        Serial.println("测试完成");
        motorTestMode = false;
        motor_stop();  // 测试完成后关闭电机
      }
    }
  }
  
  // 其他循环操作可以放在这里
  delay(10);  // 短暂延时，避免CPU过度使用
} 